
#include <math.h>

#include "globals.h"
#include "geometry.h"
#include "polygon.h"
#include "distance.h"

void
squarePointDistance(Pt *point1, Pt *point2, Real *dist)
{
	Real	xdiff, ydiff;

#ifdef DEBUG_DISTANCE
	printf ("dst: square point distance is .. ");
#endif

	xdiff = XValPtr(point1) - XValPtr(point2);
	ydiff = YValPtr(point1) - YValPtr(point2);
	*dist = (xdiff*xdiff) + (ydiff*ydiff);

#ifdef DEBUG_DISTANCE
	printf ("%f\n", *dist);
#endif
}

void
pointDistance(Pt *point1, Pt *point2, Real *dist)
{
	Real	squaredist;

#ifdef DEBUG_DISTANCE
	printf ("dst: point distance is .. ");
#endif

	squarePointDistance(point1, point2, (double *) (&squaredist));
	*dist = (Real) sqrt((double)squaredist);

#ifdef DEBUG_DISTANCE
	printf ("%f\n", *dist);
#endif
}

/*	Find the point of tangency, relative to the origin of the convex
	distance function, of the convex distance function and the 
	vertical sweepline */

void
getSweeplineTan(Pt *tanPt)
{
	/*	The Euclidean case is really easy.  The sweepline is 
		vertical and to the left of the circle, so we use the
		x-intercept of the unit circle. */

	tanPt->w = 1.0;
	tanPt->x = 1.0;
	tanPt->y = 0.0;
}

/*	For a convex distance function, if we shoot a ray from the origin
	of the distance function in the vector direction from "start"
	to "end" then we exit the convex polygon at the point "exitPt". */

void
getExitPoint(Pt *start, Pt *end, Pt *exitPt)
{
	Real	len;

#ifdef DEBUG_DISTANCE
	printf ("dst: find the exit point from "
		"%2.2f %2.2f to %2.2f %2.2f\n",
		XValPtr(start), YValPtr(start), XValPtr(end), YValPtr(end));
#endif

	/*	The Euclidean case implies that we can use the angle/slope
		of the vector from "start" to "end" as the exit point along
		the unit circle. */

	exitPt->w = 1.0; 
	exitPt->x = XValPtr(end) - XValPtr(start);
	exitPt->y = YValPtr(end) - YValPtr(start);
	vectorLength (exitPt, &len);
	exitPt->w = (Coordinate) len;

#ifdef DEBUG_DISTANCE
	printf ("dst: exit point is %2.2f %2.2f %2.2f\n", 
		exitPt->w, exitPt->x, exitPt->y);
#endif
}

/*	Find the outer common tangent that has the polygons on its left
	(ccw orientation) when going from "head" to "tail".  Necessarily,
	"start" will be a point of polygon "head" and "end" will be a 
	point of polygon "tail". */

void
outerCommonTangent(Pgon *head, Pgon *tail, Pt *start, Pt *end,
	int *headIndex, int *tailIndex)
{
	Line	compareLine;

#ifdef DEBUG_DISTANCE
	printf ("dst: compute outer common tangent.\n");
#endif

	if ((head->numPoints == 1) && (tail->numPoints == 1)) {
		PointAssign((*start), (head->points[0]));
		PointAssign((*end), (tail->points[0]));
		*headIndex = 0;
		*tailIndex = 0;
	}
	else if ((head->numPoints == 2) && (tail->numPoints == 1)) {
		PointAssign((*start), (head->points[0]));
		PointAssign((*end), (tail->points[0]));
		*headIndex = 0;
		*tailIndex = 0;
		LineOf(start, end, &compareLine);
		if (LineSide(&compareLine, &(head->points[1])) != RightSide) {
			PointAssign((*start), (head->points[1]));
			*headIndex = 1;
		}
	}
	else if ((head->numPoints == 1) && (tail->numPoints == 2)) {
		PointAssign((*start), (head->points[0]));
		PointAssign((*end), (tail->points[0]));
		*headIndex = 0;
		*tailIndex = 0;
		LineOf(start, end, &compareLine);
		if (LineSide(&compareLine, &(tail->points[1])) != RightSide) {
			PointAssign((*start), (tail->points[1]));
			*tailIndex = 1;
		}
	}
	else if ((head->numPoints == 2) && (tail->numPoints == 2)) {
		LineOf(&(head->points[0]), &(tail->points[0]), &compareLine);
		if ((LineSide(&compareLine, &(tail->points[1])) == RightSide) &&
			(LineSide(&compareLine, &(head->points[1])) == RightSide)) {
			PointAssign((*start), (head->points[0]));
			PointAssign((*end), (tail->points[0]));
			*headIndex = 0;
			*tailIndex = 0;
		}
		else {
			LineOf(&(head->points[0]), &(tail->points[1]), &compareLine);
			if ((LineSide(&compareLine, &(tail->points[0])) == RightSide) &&
				(LineSide(&compareLine, &(head->points[1])) == RightSide)) {
				PointAssign((*start), (head->points[0]));
				PointAssign((*end), (tail->points[1]));
				*headIndex = 0;
				*tailIndex = 1;
			}
			else {
				LineOf(&(head->points[1]), &(tail->points[1]), &compareLine);
				if ((LineSide(&compareLine, &(tail->points[0])) == RightSide) &&
					(LineSide(&compareLine, &(head->points[0])) == RightSide)) {
					PointAssign((*start), (head->points[1]));
					PointAssign((*end), (tail->points[1]));
					*headIndex = 1;
					*tailIndex = 1;
				}
				else {
					PointAssign((*start), (head->points[1]));
					PointAssign((*end), (tail->points[0]));
					*headIndex = 1;
					*tailIndex = 0;
				}
			}
		}
	}
	else {
		int 	ph, pt, j;
		Line	line;
		Real dist, maxDist;
		boolean	done;

		/*	Find two points, pt and ph, that are far apart on the polygons.  */

		ph = 0;
		squarePointDistance(&(head->points[0]), &(tail->points[0]), &maxDist);
		for (j = 1; j < head->numPoints; j++) {
			squarePointDistance(&(head->points[j]), &(tail->points[0]), 
					&dist);
			if (dist > maxDist) {
				maxDist = dist;
				ph = j;
			}
		}

		pt = 0;
		squarePointDistance(&(head->points[0]), &(tail->points[0]), &maxDist);
		for (j = 1; j < tail->numPoints; j++) {
			squarePointDistance(&(head->points[0]), &(tail->points[j]), 
					&dist);
			if (dist > maxDist) {
				maxDist = dist;
				pt = j;
			}
		}

		/*	At this point, I know that pt and ph are on opposite
			sides of their polygons. */
		
		done = False;
		while (done == False) {
			LineOf(&(head->points[ph]), &(tail->points[pt]), &line);
			if (LineSide(&line, &(head->points[(ph+1)%head->numPoints]))
					== RightSide)
				ph = (ph + 1) % head->numPoints;
			else if (LineSide(&line, 
				&(tail->points[(pt-1+tail->numPoints)%tail->numPoints]))
					== RightSide)
				pt = (pt-1+tail->numPoints)%tail->numPoints;
			else if (LineSide(&line, 
				&(head->points[(ph-1+head->numPoints)%head->numPoints]))
					== RightSide)
				ph = (ph-1+head->numPoints)%head->numPoints;
			else if (LineSide(&line, &(tail->points[(pt+1)%tail->numPoints]))
					== RightSide)
				pt = (pt + 1) % tail->numPoints;
			else
				done = True;
		}

		*headIndex = ph;
		*tailIndex = pt;
		PointAssignPtr(start, &(head->points[ph]));
		PointAssignPtr(end, &(tail->points[pt]));
	}
}

void
vectorLength(Pt *point, Real *length)
{
#ifdef DEBUG_DISTANCE
	printf ("dst: compute vector length of ");
#endif

	*length = (Real) sqrt((double)(point->x * point->x + 
				point->y * point->y)) / point->w;
	if (*length < 0)
		*length *= -1;

#ifdef DEBUG_DISTANCE
	printf ("%f\n", *length);
#endif
}

